#include <cv.h>
#include <highgui.h>
#include <sys/time.h>
#include "serial.h"
#include "cmucam.h"
#include "robot.h"
#include "util.h"
#include "vision.h"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/nonfree/nonfree.hpp"

using namespace cv;

#define STEP_DISTANCE 60
#define MIN_DISTANCE 20

int main(int argc, char** argv) {

	string path = "/home/pi/workspace/opencv-cam/projects/opencv-cam/samples/";
	string name = "snapshot.jpg";
	string basename = "base.jpg";
	string file = path + name;
	//string file = "./snapshot.jpg";

	TheEye eye = TheEye();
	Robot robot;

//	SiftFeatureDetector detector(400);
//	SiftDescriptorExtractor extrator;
//	vector<KeyPoint> base_keypoints;
//	vector<KeyPoint> keypoints;
//	robot.connect();
//
//	char buf[100];
//	cout << "Saving base location... Adjust the camera, then press ENTER.\n";
//	getchar();
//	eye.update();
//	sprintf(buf, "%sbase.jpg", path.c_str());
//	imwrite(buf, eye.get_current_img());
//	cout << "Base location saved. Adjust vehicle, then press ENTER.\n";
//	getchar();
//	cout << "Running...";
//
//	Mat baseImg = imread(buf);
//	detector.detect(baseImg, base_keypoints); // detects keypoints
//	Mat base_descriptors, descriptors;
//	extrator.compute(baseImg, base_keypoints, base_descriptors); // extracts descriptors
//
//	int shots_cnt = 0;
//
//	while (1) {
//		shots_cnt++;
//		if (shots_cnt >= 10) {
//			shots_cnt = 0;
//		}
//		eye.update();
//
//		Mat img = eye.get_current_img();
//		detector.detect(img, keypoints); // detects keypoints
//		extrator.compute(img, keypoints, descriptors); // extracts descriptors
//
//		// doing the matches
//		FlannBasedMatcher matcher;
//		vector<DMatch> matches;
//		matcher.match(base_descriptors, descriptors, matches);
//
//		double max_dist = 0;
//		double min_dist = 100;
//		for (int i = 0; i < base_descriptors.rows; i++) {
//			double dist = matches[i].distance;
//			if (dist < min_dist)
//				min_dist = dist;
//			if (dist > max_dist)
//				max_dist = dist;
//		}
//
//		vector<DMatch> good_matches;
//
//		for (int i = 0; i < base_descriptors.rows; i++) {
//			if (matches[i].distance <= max(2 * min_dist, 0.02)) {
//				good_matches.push_back(matches[i]);
//			}
//		}
//
//		cout << "Number of good matches: " << good_matches.size() << "\n";
//
//		Mat img_matches;
//		drawMatches(baseImg, base_keypoints, img, keypoints, good_matches,
//				img_matches);
//		sprintf(buf, "matches = %d", good_matches.size());
//		Point org = Point(EYE_X(img_matches) + 15, EYE_Y(img_matches));
//		putText(img_matches, buf, org, CV_FONT_HERSHEY_SIMPLEX, 0.5,
//				Scalar(255, 50, 50), 1, false);
//
//
//		drawKeypoints(img, keypoints, img, Scalar(R, G, B), 0);
//
//		sprintf(buf, "%s%d_snapshot.jpg", path.c_str(), shots_cnt);
//		imwrite(buf, img);
//		sprintf(buf, "%s%d_snapshot_matches.jpg", path.c_str(), shots_cnt);
//		imwrite(buf, img_matches);
//		//sprintf(buf, "%s%d_snapshot_original.jpg", path.c_str(), shots_cnt);
//		//imwrite(buf, eye.get_current_img());
//
//		cout<<"Press ENTER to continue...\n";
//		getchar();
//		sleep(3);
	int shots_cnt = 0;
	char buf[100];
	while (1) {
		eye.update();
		sprintf(buf, "%sbase.jpg", path.c_str());
		imwrite(buf, eye.get_current_img());

		Mat flooded1, src;
		eye.update();
		src = eye.get_current_img();
		flooded1.create(src.size(), src.type());
		src.copyTo(flooded1);

		int length = eye.free_space(src, flooded1);

		cout << "Free space = " << length << "\n";

		if (length > MIN_DISTANCE) {
			int time = (length / STEP_DISTANCE) + 1;
			cout << "Going forward for " << time << " secs...\n";
			robot.move(MOVE_FWD);
			sleep(time);
			robot.move(STOP);
		} else {
			while (length <= MIN_DISTANCE) {

				cout << "No space! Turning left\n";
				robot.move(LEFT_BWD);
				sleep(1);
				robot.move(STOP);

				eye.update();
				src = eye.get_current_img();
				flooded1.create(src.size(), src.type());
				src.copyTo(flooded1);
				length = eye.free_space(src, flooded1);
			}
		}
		robot.move(STOP);

		//cout << "Saving snapshots...\n";
		char buf[100];
		sprintf(buf, "%s%d_snapshot_flooded.jpg", path.c_str(), shots_cnt);
		imwrite(buf, flooded1);
		sprintf(buf, "%s%d_snapshot_original.jpg", path.c_str(), shots_cnt);
		imwrite(buf, eye.get_current_img());
		shots_cnt++;
		if (shots_cnt >= 10) {
			shots_cnt = 0;
		}

	}

	//rob.close();

	return 0;
}
